#include "project.h"
#include <stdio.h>
#include <util/crc16.h>
//#include <avr/crc16.h>
#include "crc_table.h"

#define CONSOLE_MODE 1

portTASK_FUNCTION(Task2,pvParameters) {
	DDRB = 0xff;
	while (1) {
		sbi(PORTB,7);
		vTaskDelay(500);
		cbi(PORTB,7);
		vTaskDelay(500);
	}
}

portTASK_FUNCTION(Task1,pvParameters) {
	DDRF = 0xff;
	while (1) {
		sbi(PORTB,6);
		vTaskDelay(1);
		cbi(PORTB,6);
		vTaskDelay(1);

	}
}

portTASK_FUNCTION(FSM_machine,pvParameters) {
	enum machine_state {    //enumerate declarations to simplify
		init = 'i',			//code readability of FSM state
		run = 'R',
		stop = 's',
		restart = 'r',
		quit = 'q',
		display = 'd',
		testmode = 't'
	};
	int j = 0;
	uint16_t crc = 0xffff;
	unsigned char fsmMode = init, rxModeChar = 0x00;			//, pythonInterface = 0;
	char displayTemp[300];
	while (1) {
		switch (fsmMode) {
			case init:
				//nothing to do here since initializer is preRTOS
#ifdef CONSOLE_MODE
				Usart_puts("\e[2J\e[f\n**init mode**");
				vTaskDelay(200);
#endif				//clear screen, go home
				xSerialGetChar(NULL,&rxModeChar,0);
				if (rxModeChar == 'r') {
					fsmMode = run;
					vTaskDelay(200);
					Usart_putc('r');
				}
				break;
				/**----------------------------*/
			case (run):
				xSerialGetChar(NULL,&rxModeChar,00);
				Usart_puts("\e[2J\e[f");
				avrSerialPrintf("currently in runMode. ");
				vTaskDelay(200);
				if (rxModeChar == 'd') fsmMode = display;
				if (rxModeChar == 'q') (fsmMode = quit);
				break;
				/**----------------------------*/
			case display:

				Usart_puts("\e[2J\e[f");
				Usart_puts("display mode\nEnter 20characters to perform crc16\n");
				int i = 0;
				while (((displayTemp[i - 1]) != 0x001b) && (displayTemp[i - 1]) != 0x000d) {
					if (xSerialGetChar(NULL,&displayTemp[i],0)) {
						Usart_putc(displayTemp[i++]);    //("char: '%.1s' = 0X%04X\n",
					}
				}
				Usart_putc(displayTemp[i] = NULL );
				fsmMode = testmode;

				break;
				/**----------------------------*/
			case testmode:
				crc = 0x0000;		//switch to 0xffff for modbus calc
				int timerProf = xTaskGetTickCount();
				avrSerialPrintf(" \n\n  ccurrent tick = %d",timerProf);
				for (j = 0; j < (strlen(displayTemp) - 1); j++) {    //count down because usart is MSB first.
					crc = _crc16_update(crc,displayTemp[j]);
//					crc = crcTable16(crc,displayTemp[j]);		//actually runs 30x longer

					avrSerialPrintf("\n#%d char: '%.1s' 0X%04X\tdec: %u\tASCII: ",j + 1,
									&displayTemp[j],crc,crc);
//					Usart_putc((crc & 0xff00) >> 8);
//					Usart_putc(crc & 0x00ff);
				}



//				for (j = 0; j < strlen(displayTemp); j++)
//					Usart_putc(displayTemp[j]);
				int newtimeProf = xTaskGetTickCount();
				avrSerialPrintf("\ncurrent tick = %d, difference is %d",newtimeProf,
								newtimeProf - timerProf);
				while (1)
					;

				if (rxModeChar == 'd') fsmMode = display;

				break;
				/**-------------CASE QUIT---------------*/
			case quit:
				avrSerialPrintf("\nprinting 200 chars to test the Usart speed:");
//				DDRB = 0xff;
//				PORTB = 0x00;		//shutdown the flashing output.
				for (i = 40; i < 240; ++i) {
//					vTaskDelay(1);
					Usart_putc(i);
				}
				avrSerialPrintf("\n\n******\n\nSuspending all tasks");
				vTaskSuspendAll();
				while (1){}
						//all other tasks are suspended. This spin lock is permanent.
				break;
				/**----------------------------*/
			case restart:
				avrSerialPrintf("MCU is restarting");
				fsmMode = init;
				break;
			default:
				break;
		}
		//next case for header info
		typedef struct {
			uint8_t src :8;		//source host ID
			uint8_t dst :8;		//dest host ID
			uint8_t ns :3;		//
			uint8_t nr :3;		//
			uint8_t rsv :1;		//
			uint8_t lst :1;		//last packet flag
			uint8_t op :8;		//opcode
			uint8_t len :8;		//packet length
		} hdr;

	}

}
